#include "Frame.hpp"

namespace visionhw {

Frame::Frame():_cloud(new CloudType)
{

}

void Frame::setInputFile(const std::string& jpeg, const std::string& depth)
{
    _jpeg_mat = cv::imread(jpeg);
    _depth_mat = cv::imread(depth, cv::ImreadModes::IMREAD_ANYCOLOR | cv::ImreadModes::IMREAD_ANYDEPTH);
    _depth_mat.convertTo(_depth_mat, CV_32F);
    _is_ply = false;
}

void Frame::setInputFile(const std::string& ply)
{
    pcl::io::loadPLYFile(ply, *_cloud);
    _is_ply = true;
}

void Frame::setCameraConfig(const Json::Value& intrinsics)
{
    if (_is_ply) return;
    depth_scale = intrinsics["depth_scale"].asDouble();
    ppx = intrinsics["ppx"].asDouble();
    ppy = intrinsics["ppy"].asDouble();
    fx = intrinsics["fx"].asDouble();
    fy = intrinsics["fy"].asDouble();
}

void Frame::calcCloud()
{
    if (_is_ply) return;
    _cloud->reserve(_jpeg_mat.size().height * _jpeg_mat.size().width);
    for (int r = 0; r < _jpeg_mat.rows; ++r)
    {
        for (int c = 0; c <= _jpeg_mat.cols; ++c)
        {
            _computeXYZandInsert(r, c);
        }
    }
}

void Frame::_computeXYZandInsert(int r, int c)
{
    double scene_z = static_cast<double>(_depth_mat.at<float>(r, c)) * depth_scale;
    double scene_x = (static_cast<double>(c) - ppx) * scene_z / fx;
    double scene_y = (static_cast<double>(r) - ppy) * scene_z / fy;
    _cloud->emplace_back(scene_x, scene_y, scene_z);
}

}